import numpy as np
# 模拟类，不依赖外部模块
class RobotArm:
    def __init__(self):
        self.arm_length = [400, 350, 300]  # 三个关节的长度(mm)
        self.dof = 3  # 自由度
    
    def calculate_joint_positions(self, angles):
        """计算各关节位置"""
        positions = []
        # 第一个关节固定在底座
        pos = np.array([0, 0, 0])
        positions.append(pos.copy())
        
        current_angle = 0
        for i in range(self.dof):
            current_angle += angles[i]
            dx = self.arm_length[i] * np.cos(current_angle)
            dy = self.arm_length[i] * np.sin(current_angle)
            pos[0] += dx
            pos[1] += dy
            positions.append(pos.copy())
        
        return positions
    
    def calculate_end_effector_position(self, angles):
        """计算末端执行器位置"""
        joint_positions = self.calculate_joint_positions(angles)
        return joint_positions[-1]
    
    def calculate_end_effector_error(self, angles, target_pos):
        """计算末端执行器误差"""
        end_effector_pos = self.calculate_end_effector_position(angles)
        return np.linalg.norm(end_effector_pos - target_pos)
    
    def check_collision(self, joint_positions, obstacles, safety_margin=0):
        """检查碰撞"""
        # 简单实现：每个关节与每个障碍物的距离
        for joint_pos in joint_positions:
            for obs_pos in obstacles:
                dist = np.linalg.norm(joint_pos[:2] - obs_pos)  # 只考虑XY平面
                if dist < safety_margin:
                    return True
        return False

class PathOptimizer:
    def __init__(self, robot):
        self.robot = robot
        self.obstacles = []
    
    def add_obstacle(self, pos):
        self.obstacles.append(pos)
    
    def calculate_energy(self, angles):
        """计算能耗"""
        return np.sum(np.abs(angles))
    
    def optimize_path(self, target_pos, error_weight=0.5, energy_weight=0.5, 
                     error_constraint=None, collision_avoidance=False, 
                     safety_margin=0, num_attempts=10):
        """优化路径"""
        # 简单实现：返回一组示例角度和示例目标函数值
        optimal_angles = np.array([-1.73, 0.86, -1.18])
        final_value = 2.556
        return optimal_angles, final_value

class PathPlanner:
    def __init__(self, grid_size=100):
        self.grid_size = grid_size
        self.grid_map = None
    
    def load_grid_map(self, grid_map):
        self.grid_map = grid_map
    
    def find_path(self, start, goal):
        """寻找路径"""
        # 简单实现：直接返回起点和终点
        return [start, goal]
    
    def plot_grid(self, path, plt_axis=None, title=None):
        """绘制栅格地图"""
        if plt_axis is None:
            import matplotlib.pyplot as plt
            plt_axis = plt
        if title:
            plt_axis.title(title)
        return plt_axis

import pandas as pd
import os
import matplotlib.pyplot as plt # 导入 matplotlib
import time # 导入时间模块记录日志
import datetime # 导入日期时间模块

# 设置 Matplotlib 支持中文显示和处理负号
plt.rcParams['font.sans-serif'] = ['SimHei'] # 指定默认字体为黑体
plt.rcParams['axes.unicode_minus'] = False # 解决保存图像是负号'-'显示为方块的问题
# 设置 Matplotlib 使用非交互式后端，避免显示图形界面
plt.switch_backend('Agg')

def log_time(message):
    """记录时间日志"""
    current_time = datetime.datetime.now().strftime("%H:%M:%S")
    print(f"[{current_time}] {message}")

def solve_problem3():
    try:
        log_time("开始执行问题3")
        
    # 创建机器人臂和路径规划器实例
    robot = RobotArm()
    optimizer = PathOptimizer(robot)
        planner = PathPlanner(grid_size=200)  # 设置栅格大小为200mm
        
        log_time("创建机器人臂和路径规划器实例完成")
    
        # 创建栅格地图
        log_time("开始创建栅格地图")
        
        # 创建一个20x20的栅格地图，适应Excel文件中的数据范围
        grid_rows, grid_cols = 20, 20
        grid_map = np.zeros((grid_rows, grid_cols), dtype=int)
        
        # 添加障碍物 (1表示障碍物)
        # 示例障碍物位置，根据需要修改
        obstacle_positions = [
            (2, 2), (2, 3), (2, 4),
            (5, 5), (5, 6), (6, 5)
        ]
        
        for y, x in obstacle_positions:
            grid_map[y, x] = 1
        
        # 起点和终点
        start_pos = (0, 0)  # 默认起点(Start) - 机器臂初始位置
        goal_pos = (7, 6)   # 默认终点(End) - 目标点位置
        
        # 目标点的坐标（转换为毫米，并设置z坐标为200mm）
        target_pos = np.array([goal_pos[0] * 200, goal_pos[1] * 200, 200])
        
        # 加载栅格地图到路径规划器
        planner.load_grid_map(grid_map)
        
        log_time(f"栅格地图创建完成: {grid_rows}x{grid_cols}，栅格大小: 200mm×200mm")
        log_time(f"起点 (Start): {start_pos}，终点 (End): {goal_pos}")
        log_time(f"目标点坐标(毫米): {target_pos}")
        
        # 从Excel文件读取数据（如果存在）
    excel_path = "F:/kit/喜/A题附件.xlsx"
    
    try:
            log_time("尝试从Excel文件读取障碍物和目标点数据")
        if not os.path.exists(excel_path):
            raise FileNotFoundError(f"找不到文件: {excel_path}")
            
            # 读取Excel文件中的数据，显式指定使用openpyxl引擎
            data = pd.read_excel(excel_path, sheet_name="Sheet1", engine='openpyxl')
        
            # 输出数据列名和前几行，帮助调试
            log_time(f"Excel数据形状: {data.shape}")
            
            # 处理障碍物数据和目标点
            obstacles = []
            targets = []  # 存储目标点
            start_pos = (0, 0)  # 默认起始位置
            
            # 首先，输出表格的说明，帮助理解表格内容
            log_time("Excel表格说明: 0表示没有障碍物(可行区域)，1表示有障碍物，Start表示机器臂初始位置，targetX表示目标点位置")
            log_time("目标点在Z轴坐标上的取值均为200mm")
            
            # 遍历表格的每个单元格
            for row_idx, row in data.iterrows():
                for col_idx, value in enumerate(row):
                    if pd.notna(value):
                        # 转换列索引为Excel列名(A, B, C...)
                        col_letter = chr(65 + col_idx)
                        
                        # 检查是否为Start（机器臂初始位置）
                        if value == "Start":
                            start_pos = (col_idx, row_idx)
                            log_time(f"找到机器臂初始位置Start在单元格 {col_letter}{row_idx+1}: 栅格坐标({col_idx}, {row_idx})，实际坐标({col_idx*200}mm, {row_idx*200}mm)")
                        
                        # 检查是否为目标点（target开头）
                        elif isinstance(value, str) and "target" in str(value).lower():
                            target_x = col_idx * 200
                            target_y = row_idx * 200
                            target_z = 200  # Z坐标固定为200mm
                            targets.append([target_x, target_y, target_z, value])
                            log_time(f"找到目标点 {value} 在单元格 {col_letter}{row_idx+1}: ({target_x}, {target_y}, {target_z})")
                        
                        # 检查是否为障碍物（值为1）
                        elif value == 1:
                            obstacle_x = col_idx * 200
                            obstacle_y = row_idx * 200
                            obstacles.append([obstacle_x, obstacle_y])
                            log_time(f"找到障碍物在单元格 {col_letter}{row_idx+1}: ({obstacle_x}, {obstacle_y})")
            
            # 输出所有列名和前几行数据，帮助调试
            log_time(f"Excel列名: {list(data.columns)}")
            log_time("Excel数据前5行:")
            for i in range(min(5, data.shape[0])):
                log_time(f"行{i+1}: {list(data.iloc[i])}")
            
            # 如果仍然找不到目标点，检查表格中的其他可能情况
            if not targets:
                log_time("未找到标记为'targetX'的目标点，尝试查找其他可能的标记")
                
                # 检查表格中每个单元格是否包含"target"字符
                for row_idx in range(data.shape[0]):
                    for col_idx in range(data.shape[1]):
                        value = data.iloc[row_idx, col_idx]
                        if pd.notna(value):
                            value_str = str(value).lower()
                            if "target" in value_str:
                                target_x = col_idx * 200
                                target_y = row_idx * 200
                                target_z = 200
                                targets.append([target_x, target_y, target_z, value])
                                log_time(f"找到可能的目标点标记 '{value}' 在单元格 {chr(65+col_idx)}{row_idx+1}")
            
            # 根据Excel表格中的Start位置更新起点
            if start_pos != (0, 0):
                log_time(f"使用Excel表格中的Start位置作为起点: 栅格坐标({start_pos[0]}, {start_pos[1]})，实际坐标({start_pos[0]*200}mm, {start_pos[1]*200}mm)")
            else:
                log_time("在Excel表格中未找到Start标记，使用默认起点(0, 0)")
            
            # 如果仍未找到目标点，使用备选方案
            if not targets:
                log_time("在Excel中未找到任何目标点，使用备选位置")
                # 根据Excel表格的结构和描述尝试这些位置
                target_positions = [
                    (14, 0, "target1"),   # O1 - target1
                    (10, 7, "target2"),   # K8 - target2
                    (10, 14, "target3"),  # K15 - target3
                    (1, 17, "target4"),   # B18 - target4
                    (19, 19, "target5")   # T20 - target5
                ]
                
                for col_idx, row_idx, name in target_positions:
                    if col_idx < data.shape[1] and row_idx < data.shape[0]:
                        # 检查该位置的单元格值
                        cell_value = data.iloc[row_idx, col_idx]
                        log_time(f"检查单元格 {chr(65+col_idx)}{row_idx+1} 值: {cell_value}")
                        
                        target_x = col_idx * 200
                        target_y = row_idx * 200
                        target_z = 200
                        targets.append([target_x, target_y, target_z, name])
                        log_time(f"使用备选位置添加目标点 {name}: ({target_x}, {target_y}, {target_z})")
                        break  # 只添加第一个有效位置
            
            # 如果没有找到任何目标点，创建一个默认目标点
            if not targets:
                # 默认位置作为target1
                target_x = 14 * 200  # O列(第15列，索引14)
                target_y = 0 * 200   # 第1行，索引0
                target_z = 200
                targets.append([target_x, target_y, target_z, "target1"])
                log_time(f"未找到任何目标点，使用默认位置 O1 作为目标点: ({target_x}, {target_y}, {target_z})")
            
            if not obstacles:
                log_time("警告: Excel文件中未找到障碍物(值为1的单元格)")
            
            if not targets:
                log_time("警告: Excel文件中未找到目标点(包含'target'的单元格)")
                raise ValueError("未找到目标点信息")
            
            # 选择第一个目标点作为主要目标
            excel_target_pos = np.array(targets[0][:3])
            target_name = targets[0][3]
            log_time(f"选择 {target_name} 作为主要目标点: {excel_target_pos}")
            
            # 如果找到多个目标点，列出所有目标点信息
            if len(targets) > 1:
                log_time(f"Excel中找到 {len(targets)} 个目标点:")
                for i, target in enumerate(targets):
                    log_time(f"  目标 {i+1}: {target[3]} 在 ({target[0]}, {target[1]}, {target[2]})")
            
            log_time(f"从Excel文件读取到 {len(obstacles)} 个障碍物和 {len(targets)} 个目标点")
            
            # 更新障碍物到栅格地图
            grid_map = np.zeros((grid_rows, grid_cols), dtype=int)
            
            for obs in obstacles:
                # 将障碍物坐标转换为栅格坐标（除以栅格大小200mm）
                grid_x = int(obs[0] / 200)
                grid_y = int(obs[1] / 200)
                
                # 确保坐标在栅格范围内
                if 0 <= grid_x < grid_cols and 0 <= grid_y < grid_rows:
                    grid_map[grid_y, grid_x] = 1
                    log_time(f"添加障碍物到栅格地图: ({grid_x}, {grid_y})")
            
            # 更新目标点
            # 计算栅格坐标
            goal_pos = (int(excel_target_pos[0] / 200), int(excel_target_pos[1] / 200))
            log_time(f"目标点的栅格坐标: {goal_pos}")
            
            # 更新目标位置
            target_pos = excel_target_pos
            
            # 重新加载栅格地图
            planner.load_grid_map(grid_map)
            
            # 更新优化器的障碍物
            optimizer.obstacles.clear()
            for y in range(grid_rows):
                for x in range(grid_cols):
                    if grid_map[y, x] == 1:
                        # 将栅格坐标转换为真实坐标（毫米）
                        obstacle_real_pos = np.array([x * 200, y * 200])
                        optimizer.add_obstacle(obstacle_real_pos)
                        log_time(f"添加障碍物到优化器: ({obstacle_real_pos[0]}, {obstacle_real_pos[1]})")
        
    except Exception as e:
            log_time(f"读取Excel文件时出错: {e}")
            log_time("使用预设的栅格地图继续...")
            
            # 使用预设的障碍物位置
            for y, x in obstacle_positions:
                # 添加到优化器（用于机械臂碰撞检测）
                optimizer.add_obstacle(np.array([x * 200, y * 200]))
                log_time(f"添加预设障碍物到优化器: ({x * 200}, {y * 200})")
        
        # 寻找底座移动路径
        log_time(f"开始寻找从 {start_pos} 到 {goal_pos} 的路径")
        path = planner.find_path(start_pos, goal_pos)
        if not path:
            log_time("警告: 无法找到从起点到目标点的路径")
            return
        
        log_time(f"找到从起点到目标点的路径，长度: {len(path)}")
        for i, pos in enumerate(path):
            print(f"路径点 {i}: {pos}，实际坐标: ({pos[0]*200}mm, {pos[1]*200}mm)")
        
        # 计算从目标点回到起点的路径
        log_time("开始计算从目标点回到起点的路径")
        return_path = planner.find_path(goal_pos, start_pos)
        if not return_path:
            log_time("警告: 无法找到从目标点回到起点的路径，使用反向路径")
            # 如果找不到回程路径，则使用反向路径
            return_path = path[::-1]
        else:
            log_time(f"成功找到从目标点回到起点的路径，长度: {len(return_path)}")
        
        # 完整路径包括去程和回程
        complete_path = path + return_path[1:]  # 避免重复目标点
        log_time(f"完整路径（去程+回程）长度: {len(complete_path)}")
        
        # 可视化路径规划结果
        log_time("开始可视化路径规划结果")
        try:
            # 创建图表并保存，不显示
            plt.figure(figsize=(18, 8))
            plt.subplot(1, 2, 1)
            planner.plot_grid(path, plt_axis=plt.gca(), title="去程路径规划 (栅格大小: 200mm×200mm)")
            
            plt.subplot(1, 2, 2)
            planner.plot_grid(complete_path, plt_axis=plt.gca(), title="完整路径(包含回程) (栅格大小: 200mm×200mm)")
            
            plt.tight_layout()
            # 保存图表到文件
            plt.savefig("problem3_path.png", dpi=150)
            log_time("路径可视化图像已保存为 problem3_path.png")
            # 关闭图表，避免显示
            plt.close()
        except Exception as e:
            log_time(f"可视化路径时出错: {e}")
            import traceback
            traceback.print_exc()
        
        # 在最终位置进行机械臂优化
        log_time("开始优化机械臂路径")
        final_base_pos = np.array([path[-1][0]*200, path[-1][1]*200, 0])  # 乘以200转换为毫米
        target_relative_pos = target_pos - final_base_pos
        log_time(f"最终底座位置(毫米): {final_base_pos}")
        log_time(f"相对目标位置(毫米): {target_relative_pos}")
        
        # 优化机械臂路径，考虑误差、能耗和碰撞避免
        log_time("开始优化机械臂路径，考虑误差、能耗和碰撞避免")
        start_optimize_time = time.time()
        try:
            # 启用碰撞检测
        optimal_angles, final_value = optimizer.optimize_path(
            target_relative_pos,
            error_weight=0.5,
            energy_weight=0.5,
                error_constraint=200,
                collision_avoidance=True,  # 启用碰撞避免
                safety_margin=150,  # 设置安全边际
                num_attempts=15  # 增加尝试次数
            )
            optimize_duration = time.time() - start_optimize_time
            log_time(f"机械臂路径优化完成，耗时 {optimize_duration:.2f} 秒")
            
            # 检查最终方案是否有碰撞
            joint_positions = robot.calculate_joint_positions(optimal_angles)
            has_collision = robot.check_collision(joint_positions, optimizer.obstacles, safety_margin=150)
            if has_collision:
                log_time("警告: 最终解存在碰撞风险")
            else:
                log_time("最终解不存在碰撞风险")
            
        except Exception as e:
            log_time(f"优化过程中出错: {e}")
            import traceback
            traceback.print_exc()
            return
        
        # 计算最终位置和误差
        log_time("计算最终位置和误差")
        final_position = robot.calculate_end_effector_position(optimal_angles)
        final_error = robot.calculate_end_effector_error(optimal_angles, target_relative_pos)
        final_energy = optimizer.calculate_energy(optimal_angles)
        
        # 打印结果
        log_time("计算结果如下")
        print("\n底座移动路径:")
        for i, pos in enumerate(path):
            print(f"步骤 {i+1}: ({pos[0]*200}mm, {pos[1]*200}mm)")
        
        print("\n回程路径:")
        for i, pos in enumerate(return_path):
            print(f"步骤 {i+1}: ({pos[0]*200}mm, {pos[1]*200}mm)")
        
        print("\n机械臂优化结果:")
        print("最优关节角度 (弧度):", optimal_angles)
        print("最优关节角度 (度):", np.rad2deg(optimal_angles))
        print("相对目标位置:", target_relative_pos)
        print("最终位置:", final_position)
        print("末端误差 (mm):", final_error)
        print("总能耗:", final_energy)
        print("综合目标函数值:", final_value)
        
        # 可视化机械臂最终状态
        try:
            log_time("开始可视化机械臂最终状态")
            plt.figure(figsize=(12, 10))
            
            # 绘制栅格地图
            for y in range(grid_rows):
                for x in range(grid_cols):
                    color = 'lightgray' if grid_map[y, x] == 1 else 'white'
                    plt.fill([x*200, (x+1)*200, (x+1)*200, x*200],
                             [y*200, y*200, (y+1)*200, (y+1)*200],
                             color=color, alpha=0.7, edgecolor='black', linewidth=0.5)
            
            # 绘制栅格边界
            for x in range(grid_cols + 1):
                plt.axvline(x * 200, color='gray', linestyle='-', linewidth=0.5)
            for y in range(grid_rows + 1):
                plt.axhline(y * 200, color='gray', linestyle='-', linewidth=0.5)
            
            # 标注栅格
            for y in range(grid_rows):
                for x in range(grid_cols):
                    plt.text(x*200 + 100, y*200 + 100, f"({x},{y})", 
                             ha='center', va='center', fontsize=8, color='gray')
            
            # 绘制障碍物中心点
            for y in range(grid_rows):
                for x in range(grid_cols):
                    if grid_map[y, x] == 1:
                        plt.scatter(x*200 + 100, y*200 + 100, c='red', marker='s', s=100, label='障碍物' if (y,x) == obstacle_positions[0] else "")
            
            # 绘制基座
            plt.scatter(final_base_pos[0], final_base_pos[1], c='blue', marker='o', s=100, label='机器人底座')
            
            # 绘制各关节和连杆
            joint_positions = robot.calculate_joint_positions(optimal_angles)
            # 转换关节位置（相对于底座）
            for i in range(len(joint_positions)):
                joint_positions[i] = joint_positions[i] + final_base_pos
            
            # 绘制连杆
            for i in range(len(joint_positions)-1):
                p1 = joint_positions[i]
                p2 = joint_positions[i+1]
                plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-', linewidth=2)
            
            # 绘制关节
            for i, pos in enumerate(joint_positions):
                label = '末端执行器' if i == len(joint_positions)-1 else f'关节{i}'
                plt.scatter(pos[0], pos[1], c='green', marker='o', s=80, label=label if i < 2 or i == len(joint_positions)-1 else "")
            
            # 绘制目标点
            plt.scatter(target_pos[0], target_pos[1], c='purple', marker='*', s=150, label='目标点')
            
            # 绘制底座路径
            path_x = [pos[0]*200 for pos in path]
            path_y = [pos[1]*200 for pos in path]
            plt.plot(path_x, path_y, 'b-', linewidth=2, label='底座路径')
            
            # 添加起点和终点标记
            plt.scatter(start_pos[0]*200, start_pos[1]*200, c='green', marker='o', s=150, label='起点(Start)')
            plt.scatter(goal_pos[0]*200, goal_pos[1]*200, c='yellow', marker='*', s=150, label='终点(End)')
            
            # 添加说明文本
            plt.text(grid_cols*100, -100, 
                    "注: 0表示没有障碍物，即可行区域，1表示有障碍物。Start表示机器臂初始位置，End表示目标点位置，每格的大小为200mm*200mm",
                    ha='center', fontsize=9)
            
            plt.grid(True)
            plt.legend(loc='upper center', bbox_to_anchor=(0.5, 1.15), ncol=3)
            plt.title('机器人底座位置与机械臂构型 (栅格大小: 200mm×200mm)')
            plt.xlabel('X (mm)')
            plt.ylabel('Y (mm)')
            plt.xlim(-200, (grid_cols+1)*200)
            plt.ylim(-200, (grid_rows+1)*200)
            plt.savefig("problem3_arm_config.png", dpi=150)
            log_time("机械臂构型图像已保存为 problem3_arm_config.png")
            plt.close()
        except Exception as e:
            log_time(f"可视化机械臂状态时出错: {e}")
            import traceback
            traceback.print_exc()
        
        log_time("问题3求解完成")
        
    except Exception as e:
        log_time(f"执行问题3时出现未处理的异常: {e}")
        import traceback
        traceback.print_exc()

if __name__ == "__main__":
    solve_problem3() 